package com.seventh.blekit.fix0629;

import android.util.Log;

import androidx.annotation.NonNull;
import androidx.annotation.Size;

import java.util.Arrays;

/**
 * @author chengxin
 * create at 2021/5/21
 * description:重新解析传感器数据
 */
public class SensorDef {
    // 1位
    //电池电量
    public float power;
    // 2-7位
    public float gyrosX;
    public float gyrosY;
    public float gyrosZ;
    // 8-13位
    public float accelX;
    public float accelY;
    public float accelZ;

    // 14-19位
    public float magX;
    public float magY;
    public float magZ;
    public float q0,q1,q2,q3;
    public float q7[];
    public int buttonflag;

    public static SensorDef create(@Size(min=19,max=31) byte[] value) {
        SensorDef def = new SensorDef();
        if(value[0]==0x56){
            def.buttonflag = 1;
        }else {
            def.buttonflag = 0;
        }
        def.power = value[1];
        def.q0 = func(value[2], value[3])/10000f;
        def.q1 = func(value[4], value[5])/10000f;
        def.q2 = func(value[6], value[7])/10000f;
        def.q3 = func(value[8], value[9])/10000f;


        def.gyrosX = func(value[10], value[11])/100f;
        def.gyrosY = func(value[12], value[13])/100f;
        def.gyrosZ = func(value[14], value[15])/100f;



        if(value.length>19&&value.length<28){
            def.accelX = func(value[16], value[17])/100f;
            def.accelY = func(value[18], value[19])/100f;
            def.accelZ = func(value[20], value[21])/100f;

    }



        return def;
    }

    private float[] getQ7() {
        //return NewCal.MahonyQuaternionUpdate(accelX, accelY, accelZ, gyrosX, gyrosY, gyrosZ, magX, magY, magZ);
        return Parser.mpu_update(gyrosX, gyrosY, gyrosZ, accelX, accelY, accelZ, magX, magY, magZ);
    }

    public String createUnityMsg() {
        return q0 + "," + q1 + "," + q2 + "," + q3 + "," + gyrosX + "," + gyrosY + "," + gyrosZ+ "," + accelX+ "," + accelY+ "," + accelZ+ "," + buttonflag+","+power;
    }

    public void printQ4() {
        Log.e("print", Arrays.toString(q7));
    }

    @NonNull
    @Override
    public String toString() {

        return "power:" +
                power +
                "%,\n" +
                "gyrosX:" +
                gyrosX +
                ",\n" +
                "gyrosY:" +
                gyrosY +
                ",\n" +
                "gyrosZ:" +
                gyrosZ +
                ",\n" +
                "accelX:" +
                accelX +
                ",\n" +
                "accelY:" +
                accelY +
                ",\n" +
                "accelZ:" +
                accelZ +
                ",\n" +
                "magX:" +
                magX +
                ",\n" +
                "magY:" +
                magY +
                ",\n" +
                "magZ:" +
                magZ;

    }

    /**
     * 第一位带符号，第二位不带符号
     */
    public static float func(byte bt0, byte bt1) {
        byte a = (byte) (bt0 & 0x80);
        int reset = Integer.parseInt(byteToString(bt1), 16);
        if (a != 0) {
            byte bt00 = (byte) (bt0 & 0x7f);
            return -((float) (bt00 * 256 + reset)) ;
        } else {
            return ((float) (bt0 * 256 + reset)) ;
        }
    }

    private static String byteToString(byte value) {
        String hex = Integer.toHexString(value & 0xFF);
        if (hex.length() < 2) {
            hex = "0" + hex;
        }
        return hex;
    }
}
